close all;
clear;

try
    % 获取当前文件所在目录
    current_dir = fileparts(mfilename('fullpath'));
    % 将当前目录添加到搜索路径
    addpath(current_dir);
    
    % 检查库是否已加载，如果已加载则先卸载
    if libisloaded('NET_AMC3XER')
        unloadlibrary('NET_AMC3XER');
    end
    
    % 参数设定
    steps_per_rev = 10000;   % 步进电机一圈步数
    gear_ratio = 40;         % 减速比
    destIP = '192.168.1.30'; % 控制卡IP
    axis = 0;                % 电机轴号（假设为0）
    
    % 用户输入（带默认值）
    default_amp = 30;
    default_speed = 1;
    default_n_cycles = 20;
    default_adc_chan = 0;
    default_supply_current = 0;
    
    if evalin('base', 'exist(''amp'', ''var'')'), amp = evalin('base', 'amp'); else
        amp = input(sprintf('请输入三角波幅值（单位：度，默认%d）：', default_amp));
        if isempty(amp), amp = default_amp; end
    end
    if evalin('base', 'exist(''speed'', ''var'')'), speed = evalin('base', 'speed'); else
        speed = input(sprintf('请输入三角波速度（单位：度/秒，默认%d）：', default_speed));
        if isempty(speed), speed = default_speed; end
    end
    if evalin('base', 'exist(''n_cycles'', ''var'')'), n_cycles = evalin('base', 'n_cycles'); else
        n_cycles = input(sprintf('请输入三角波周期数（默认%d）：', default_n_cycles));
        if isempty(n_cycles), n_cycles = default_n_cycles; end
    end
    if evalin('base', 'exist(''adc_chan'', ''var'')'), adc_chan = evalin('base', 'adc_chan'); else
        adc_chan = input(sprintf('请输入ADC采集通道（0-7，默认%d）：', default_adc_chan));
        if isempty(adc_chan), adc_chan = default_adc_chan; end
    end
    if evalin('base', 'exist(''supply_current'', ''var'')'), supply_current = evalin('base', 'supply_current'); else
        supply_current = input(sprintf('请输入供电电流（A，默认%d）：', default_supply_current));
        if isempty(supply_current), supply_current = default_supply_current; end
    end
    
    % 打印参数设置
    fprintf('参数设置：\n');
    fprintf('  三角波幅值：%g 度\n', amp);
    fprintf('  三角波速度：%g 度/秒\n', speed);
    fprintf('  三角波周期数：%d\n', n_cycles);
    fprintf('  ADC通道：%d\n', adc_chan);
    fprintf('  供电电流：%g A\n', supply_current);
    
    % ========== 新增：电源控制对象和自动清理 ========== 
    dll_path = fullfile(current_dir, 'ATK-DP100DLL(x64).dll');
    NET.addAssembly(dll_path);
    api = ATK_DP100DLL.ATKDP100API();
    index = uint8(1);
    current_set = uint16(supply_current * 1000); % mA
    voltage_set = uint16(19000); % 19V = 19000mV
    ocp = current_set;
    ovp = voltage_set;
    % 定义清理函数，确保退出时关闭输出和断开设备
    cleanupObj = onCleanup(@() safePowerClose(api, index, current_set, voltage_set));
    try
        % 连接设备
        if ~api.DevOpenOrClose()
            error('电源连接失败');
        end
        % 获取设备信息（可选，便于调试）
        [~, devtype, hdwVer, appver, devsn, devState] = api.GetDevInfo('', '', '', '', '');
        fprintf('>>> 设备类型: %s, 硬件版本: %s, APP版本: %s, SN: %s, 状态: %s\n', devtype, hdwVer, appver, devsn, devState);
        % 设置参数
        if ~api.SetGroupInfo(index, current_set, voltage_set, ocp, ovp)
            error('设置电源输出参数失败');
        end
        if ~api.UseGroup(index, current_set, voltage_set, ocp, ovp)
            error('调出1号组失败');
        end
        if ~api.OpenOut(index, current_set, voltage_set)
            error('电源输出打开失败');
        end
        fprintf('电源已设置为19V, 电流%gA, 并已打开输出\n', supply_current);
        pause(2); % 等待2秒让电流稳定
    catch powerErr
        fprintf('电源设置或运动控制失败：%s\n', powerErr.message);
    end
    % ========== 电源控制结束 ========== 
    
    % 验证输入
    if adc_chan < 0 || adc_chan > 7
        error('ADC通道必须在0-7之间');
    end
    
    % 三角波参数
    sample_interval = 0.001; % 采样间隔(s)，1000Hz
    ui_update_interval = 0.05; % UI每50ms刷新一次
    last_ui_update_time = tic;
    
    % 角度转步数/步数转角度
    angle2step = @(angle) round(angle * steps_per_rev * gear_ratio / 360);
    step2angle = @(step) step * 360 / (steps_per_rev * gear_ratio);
    
    % 初始化库和设备
    fprintf('正在加载驱动库...\n');
    % 加载库
    loadlibrary('NET_AMC3XER','NET_AMC3XER.h');
    
    % 初始化设备
    fprintf('正在初始化设备...\n');
    init_result = calllib('NET_AMC3XER', 'SOCKET_init');
    if init_result ~= 0
        error('设备初始化失败，错误代码：%d', init_result);
    end
    
    % 设置轴
    axis_result = calllib('NET_AMC3XER', 'Set_Axs', destIP, axis, 1, 0, 0, 0);
    if axis_result ~= 0
        error('轴设置失败，错误代码：%d', axis_result);
    end
    
    % 编码器初始化（4x模式，Z信号不复位，复位为0，enable先0后1）
    calllib('NET_AMC3XER', 'Set_Encorder', destIP, int32(axis), int32(3), int32(0), int32(0), int32(0), int32(0));
    calllib('NET_AMC3XER', 'Set_Encorder', destIP, int32(axis), int32(3), int32(0), int32(0), int32(0), int32(1));
    
    % 数据存储
    max_samples = 10000 * n_cycles * 3; % 预分配足够空间
    enc_data = zeros(max_samples,1);
    adc_data = zeros(max_samples,1);
    time_data = zeros(max_samples,1);
    enc_data_offset = zeros(max_samples,1); % 新增：记录相对初始值
    sample_idx = 1;
    
    fprintf('开始三角波运动与数据采集...\n');
    current_angle = 0;
    enc_data0 = [];
    t_start = tic;
    for half_cycle = 1:(n_cycles*2)
        % 判断奇偶半周期
        if mod(half_cycle,2)==1
            target_angle = amp;
        else
            target_angle = 0;
        end
        delta_angle = target_angle - current_angle;
        delta_step = angle2step(delta_angle);
        if delta_step >= 0
            dir = 0;
        else
            dir = 1;
        end
        length_move = abs(delta_step);
        
        % DeltMov参数设置
        curve = 1;           % S曲线加减速
        outmod = 0;         % 脉冲输出规格
        vo = 1000;          % 初始速度 1000 PPS
        vt = min(250000, abs(speed * steps_per_rev * gear_ratio / 360)); % 运行速度，根据输入的度/秒换算为PPS
        start_dec = 0;      % 自动设定减速位置
        acc_time = 10;     % 加速时间 100ms
        dec_time = 10;     % 减速时间 100ms
        sd_en = 0;          % 不使用手动减速
        wait_sync = 0;      % 立即启动
        
        if length_move > 0
            mov_result = calllib('NET_AMC3XER', 'DeltMov', destIP, axis, curve, dir, ...
                outmod, vo, vt, length_move, start_dec, acc_time, dec_time, sd_en, wait_sync);
            if mov_result == -1
                error('电机运动失败：通信失败，需要重新打开设备');
            end
        end
        
        % 运动监控与实时采样
        while true
            % 读取编码器（Read_Position）
            pos_val = uint32(0);
            runstate = uint8(0);
            iostate = uint8(0);
            cemg = uint8(0);
            pos_ptr = libpointer('uint32Ptr', pos_val);
            runstate_ptr = libpointer('uint8Ptr', runstate);
            iostate_ptr = libpointer('uint8Ptr', iostate);
            cemg_ptr = libpointer('uint8Ptr', cemg);
            enc_result = calllib('NET_AMC3XER', 'Read_Position', destIP, uint32(axis), pos_ptr, runstate_ptr, iostate_ptr, cemg_ptr);
            if enc_result == -1
                error('编码器读取失败，通信错误');
            end
            enc_val = bitand(pos_ptr.Value, hex2dec('FFFFFF'));
            if isempty(enc_data0)
                enc_data0 = enc_val; % 记录初始值
            end
            enc_data_offset(sample_idx) = enc_val - enc_data0;
            
            % 读取ADC
            adc_val = single(0);
            adc_ptr = libpointer('singlePtr', adc_val);
            adc_result = calllib('NET_AMC3XER', 'ADSingle', destIP, adc_chan, adc_ptr);
            if adc_result ~= 0
                error('ADC读取失败，错误代码：%d', adc_result);
            end
            adc_data(sample_idx) = adc_ptr.Value;
            
            % 记录时间
            time_data(sample_idx) = toc(t_start);
            
            % 检查RunState
            if runstate_ptr.Value == 0
                break;
            end
            sample_idx = sample_idx + 1;
            pause(sample_interval);
        end
        current_angle = target_angle;
        % 显示进度
        fprintf('完成进度：%.1f%%\n', half_cycle/(n_cycles*2)*100);
        % pause(0.05);
        
    end
    
    % 截取有效数据
    enc_data = enc_data(1:sample_idx-1);
    adc_data = adc_data(1:sample_idx-1);
    time_data = time_data(1:sample_idx-1);
    enc_data_offset = enc_data_offset(1:sample_idx-1);
    
    % 保存数据为CSV
    % 构建合法文件名和创建data文件夹（用绝对路径）
    data_dir = fullfile(current_dir, 'data');
    if ~exist(data_dir, 'dir')
        mkdir(data_dir);
    end
    % 替换小数点为下划线，防止文件名非法
    current_str = strrep(num2str(supply_current),'.','_');
    amp_str = strrep(num2str(amp),'.','_');
    speed_str = strrep(num2str(speed),'.','_');
    ncycles_str = num2str(n_cycles);
    csv_file = fullfile(data_dir, sprintf('triangle_wave_data_%sA_%sdeg_%sdps_%scycles.csv', current_str, amp_str, speed_str, ncycles_str));
    torque_data = adc_data * 10; % 电压转扭矩
    T = table(time_data, step2angle(enc_data_offset), torque_data, ...
        'VariableNames', {'Time_s','EncoderAngle_deg','Torque_Nm'});
    
    % --- 周期分割与峰值提取 ---
    encoder_angle = step2angle(enc_data_offset);
    % 以角度回到0附近为周期分界（允许一定误差）
    zero_crossings = find(abs(encoder_angle) < 1); % 1度容差
    % 只保留周期起点（避免重复）
    zero_crossings = [1; zero_crossings(diff([0;zero_crossings])>10)];
    % 确保周期数不超过n_cycles
    if length(zero_crossings) > n_cycles+1
        zero_crossings = zero_crossings(1:n_cycles+1);
    end
    % 若分界点不足，补全
    while length(zero_crossings) < n_cycles+1
        zero_crossings = [zero_crossings; length(encoder_angle)];
    end
    % 每周期找峰值
    pos_peak_time = nan(n_cycles,1); pos_peak_val = nan(n_cycles,1);
    neg_peak_time = nan(n_cycles,1); neg_peak_val = nan(n_cycles,1);
    for k = 1:n_cycles
        idx1 = zero_crossings(k); idx2 = zero_crossings(k+1)-1;
        if idx2 > length(torque_data), idx2 = length(torque_data); end
        [pmax, imax] = max(torque_data(idx1:idx2));
        [pmin, imin] = min(torque_data(idx1:idx2));
        pos_peak_val(k) = pmax;
        pos_peak_time(k) = time_data(idx1+imax-1);
        neg_peak_val(k) = pmin;
        neg_peak_time(k) = time_data(idx1+imin-1);
    end
    % 补齐到主表长度
    nrows = height(T);
    pos_locs_pad = nan(nrows,1); pos_peaks_pad = nan(nrows,1);
    neg_locs_pad = nan(nrows,1); neg_peaks_pad = nan(nrows,1);
    pos_locs_pad(1:n_cycles) = pos_peak_time;
    pos_peaks_pad(1:n_cycles) = pos_peak_val;
    neg_locs_pad(1:n_cycles) = neg_peak_time;
    neg_peaks_pad(1:n_cycles) = neg_peak_val;
    T.Time_PosPeak_s = pos_locs_pad;
    T.Torque_PosPeak_Nm = pos_peaks_pad;
    T.Time_NegPeak_s = neg_locs_pad;
    T.Torque_NegPeak_Nm = neg_peaks_pad;
    writetable(T, csv_file);
    
    % 运动结束后绘制一次图像
    fig_handle = figure('Name', '三角波运动与扭矩采样结果', 'NumberTitle', 'off', 'Position', [100 100 1200 600]);
    subplot(2,2,1);
    plot(time_data, encoder_angle, 'b-'); hold on;
    plot(time_data, encoder_angle(1)*ones(size(time_data)), 'r--');
    title('角度实时监控'); xlabel('时间 (s)'); ylabel('角度 (度)'); legend('实际角度','目标角度'); grid on;
    subplot(2,2,2);
    plot(time_data, torque_data, 'k-'); hold on;
    % 绘制每周期正负峰值连线
    plot(pos_peak_time, pos_peak_val, 'r.-', 'LineWidth', 1.5, 'MarkerSize', 12);
    plot(neg_peak_time, neg_peak_val, 'b.-', 'LineWidth', 1.5, 'MarkerSize', 12);
    title('扭矩实时监控'); xlabel('时间 (s)'); ylabel('扭矩 (Nm)'); grid on;
    subplot(2,2,3:4);
    scatter(encoder_angle, torque_data, 10, 'filled');
    title('角度-扭矩关系'); xlabel('编码器角度 (度)'); ylabel('扭矩 (Nm)'); grid on;
    
    % 自动保存图像为png，与csv同名
    [~, name, ~] = fileparts(csv_file);
    png_file = fullfile(data_dir, [name, '.png']);
    saveas(fig_handle, png_file);
    
    fprintf('运动与采集完成，数据已保存为 %s\n', csv_file);
    
catch ME
    % 错误处理
    fprintf('发生错误：%s\n', ME.message);
    fprintf('错误详细信息：\n');
    disp(ME.getReport);
end

% 清理
try
    if libisloaded('NET_AMC3XER')
        calllib('NET_AMC3XER', 'SOCKET_delete');
        unloadlibrary('NET_AMC3XER');
    end
catch cleanup_error
    fprintf('清理过程中发生错误：%s\n', cleanup_error.message);
end

% ========== 电源安全关闭函数 ========== 
function safePowerClose(api, index, iset, vset)
    try
        api.CloseOut(index, iset, vset);
        api.DevOpenOrClose();
        disp('>>> 电源输出已关闭，设备已断开');
    catch
        disp('>>> 设备关闭时发生异常（可能已断开）');
    end
end 